#pragma once
#ifndef MULTI_ROBOT_COLLLISION_DETECTOR_H
#define MULTI_ROBOT_COLLLISION_DETECTOR_H

#include "basic_typedef.h"
#include "CollisionDetector.h"

class MultiRobotCollisionDetector
{
public:
    MultiRobotCollisionDetector(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles);
    ~MultiRobotCollisionDetector(void);

    bool valid_conf(vector<Conf> &pos);
    bool local_planner(vector<Conf> &start, vector<Conf> &target, double eps = 1);

    vector<CollisionDetector*>::const_iterator begin();
    vector<CollisionDetector*>::const_iterator end();

private:

    bool is_colliding(vector<Conf> &start, vector<Conf> &target, double eps = 1);

    vector<Polygon_2>           &m_robot_polygons;
    int                         m_robot_num;
    Obstacles                   &m_obstacles;           // polygonal obstacles + scenario boundaries
    vector<CollisionDetector*>	m_cols;                 // Collision detectors for each robot with obstacles
    vector<vector<CollisionDetector*>> m_colsRobots;    // Collision detectors for each robot with other robots
};

#endif